/*
 * motor.c
 *
 *  Created on: Feb 26, 2013
 *      Author: ostan
 */

#include <avr/io.h>
#include <stdlib.h>

static volatile uint8_t motorSpeed[6];

/*
 Motor0
 PB3 (enable high)
 PA3

 Motor1
 PG2 (enable high)
 PA2

 Motor2
 PB4
 PA5

 Motor3
 PB6
 PA4

 Motor4
 PB5
 PA7

 Motor5
 PB7
 PA6


 */

void Motors_init(void) {
	// setup PWM outputs
	DDRB |= 1 << DDB3 | 1 << PB4 | 1 << PB5 | 1 << PB6 | 1 << PB7;
	DDRG |= 1 << DDG2;

	// set direction outputs
	DDRA |= 1 << DDA2 | 1 << DDA3 | 1 << DDA4 | 1 << DDA5 | 1 << DDA6
			| 1 << DDA7;
}

void Motor_task(void) {
	if (motorSpeed[0]>0) PORTB |= (1 << PB3);
	if (motorSpeed[1]>0) PORTG |= (1 << PG2);
	if (motorSpeed[2]>0) PORTB |= (1 << PB4);
	if (motorSpeed[3]>0) PORTB |= (1 << PB6);
	if (motorSpeed[4]>0) PORTB |= (1 << PB5);
	if (motorSpeed[5]>0) PORTB |= (1 << PB7);
}

void SetMotorSpeed(uint8_t i, int8_t speed) {
	if (i > 5) return;
	if (i == 0) {
		motorSpeed[0] = abs(speed);
		if (speed == 0)
			PORTB &= ~(1 << PB3); // enable1 = 0
		else if (speed > 0)
			PORTA |= 1 << PA3; // input1  = 1
		else
			PORTA &= ~(1 << PA3); // input1  = 0
	}

	else if (i == 1) {
		motorSpeed[1] = abs(speed);
		if (speed == 0)
			PORTG &= ~(1 << PG2); // enable2 = 0
		else if (speed > 0)
			PORTA |= 1 << PA2; // input2  = 1
		else
			PORTA &= ~(1 << PA2); // input2  = 0
	}

	else if (i == 2) {
		motorSpeed[2] = abs(speed);
		if (speed == 0)
			PORTB &= ~(1 << PB4); // enable3 = 0
		else if (speed > 0)
			PORTA |= 1 << PA5; // input3  = 1
		else
			PORTA &= ~(1 << PA5); // input3  = 0
	} else if (i == 3) {
		motorSpeed[3] = abs(speed);
		if (speed == 0)
			PORTB &= ~(1 << PB6); // enable4 = 0
		else if (speed > 0)
			PORTA |= 1 << PA4; // input4  = 1
		else
			PORTA &= ~(1 << PA4); // input4  = 0
	}

	else if (i == 4) {
		motorSpeed[4] = abs(speed);
		if (speed == 0)
			PORTB &= ~(1 << PB5); // enable5 = 0
		else if (speed > 0)
			PORTA |= 1 << PA7; // input5  = 1
		else
			PORTA &= ~(1 << PA7); // input5  = 0
	}

	else if (i == 5) {
		motorSpeed[5] = abs(speed);
		if (speed == 0)
			PORTB &= ~(1 << PB7); // enable6 = 0
		else if (speed > 0)
			PORTA |= 1 << PA6; // input6  = 1
		else
			PORTA &= ~(1 << PA6); // input6  = 0
	}
}

